#include <string.h>
#include <stdio.h>
#include <assert.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <vector>
#include "stdint.h"

#define USAR1  "/dev/robot_base"
#define DI 0x07

uint8_t buf[255] = {0};

void set_baudrate (struct termios *opt, unsigned int baudrate);
void set_data_bit (struct termios *opt, unsigned int databit);
void set_stopbit (struct termios *opt, const char *stopbit);
void set_parity (struct termios *opt, char parity);
int set_port_attr (int fd,int  baudrate, int  databit, const char *stopbit, char parity, int vtime,int vmin );

void DataLinkParse(uint8_t *buf, int length);
void ResetRxState(void);
bool VerifyOneFrame(void);
void InitCrc8(void);
void Crc8(uint8_t *crc, const uint8_t m);
uint8_t CalBufCrc8(const uint8_t *inBuf, uint32_t inBufSz);

void set_baudrate(struct termios *opt, unsigned int baudrate)
{
    cfsetispeed(opt, baudrate);
    cfsetospeed(opt, baudrate);
}
 
void set_stopbit(struct termios *opt, const char *stopbit)
{
    if (0 == strcmp (stopbit, "1")) {
        opt->c_cflag &= ~CSTOPB;                                                           /* 1位停止位t          */
    }else if (0 == strcmp (stopbit, "1.5")) {
        opt->c_cflag &= ~CSTOPB;                                                           /* 1.5位停止位         */
    }else if (0 == strcmp (stopbit, "2")) {
        opt->c_cflag |= CSTOPB;                                                            /* 2 位停止位          */
    }else {
        opt->c_cflag &= ~CSTOPB;                                                           /* 1 位停止位          */
    }
}
 
void set_data_bit(struct termios *opt, unsigned int databit)
{
    opt->c_cflag &= ~CSIZE;
    switch (databit) {
    case 8:
        opt->c_cflag |= CS8;
    break;
    case 7:
        opt->c_cflag |= CS7;
    break;
    case 6:
        opt->c_cflag |= CS6;
    break;
    case 5:
        opt->c_cflag |= CS5;
    break;
    default:
        opt->c_cflag |= CS8;
    break;
    }
}
 
void set_parity(struct termios *opt, char parity)
{
    switch (parity) {
    case 'N':                                                                                   /* 无校验          */
    case 'n':
        opt->c_cflag &= ~PARENB;
        break;
    case 'E':                                                                                   /* 偶校验          */
    case 'e':
        opt->c_cflag |= PARENB;
        opt->c_cflag &= ~PARODD;
        break;
    case 'O':                                                                                   /* 奇校验           */
    case 'o':
        opt->c_cflag |= PARENB;
        opt->c_cflag |= ~PARODD;
        break;
    case 'S':
    case 's':
        opt->c_cflag &= ~PARENB;                                                                 /*清除校验位   disable pairty checking Space校验  */
        opt->c_cflag &= ~CSTOPB;        
        opt->c_iflag |= INPCK;   
        break;
    default:                                                                                    /* 其它选择为无校验 */
        opt->c_cflag &= ~PARENB;
        break;
    }
}

int  set_port_attr(int fd,int  baudrate, int  databit, const char *stopbit, char parity, int vtime,int vmin )
{
    struct termios opt;
    tcgetattr(fd, &opt);       //获取初始设置
        
    set_baudrate(&opt, baudrate);
    set_data_bit(&opt, databit);
    set_parity(&opt, parity);
    set_stopbit(&opt, stopbit);
        
    opt.c_cflag &= ~CRTSCTS;   // 不使用硬件流控制
    opt.c_cflag |= CLOCAL | CREAD; //CLOCAL--忽略 modem 控制线,本地连线, 不具数据机控制功能, CREAD--使能接收标志 
 
    /*
    IXON--启用输出的 XON/XOFF 流控制
    IXOFF--启用输入的 XON/XOFF 流控制
    IXANY--允许任何字符来重新开始输出
    IGNCR--忽略输入中的回车
    */
    opt.c_iflag &= ~(BRKINT | ICRNL | INPCK | ISTRIP | IXON);
    opt.c_oflag &= ~OPOST; //启用输出处理
    /*
    ICANON--启用标准模式 (canonical mode)。允许使用特殊字符 EOF, EOL,
        EOL2, ERASE, KILL, LNEXT, REPRINT, STATUS, 和 WERASE，以及按行的缓冲。
    ECHO--回显输入字符
    ECHOE--如果同时设置了 ICANON，字符 ERASE 擦除前一个输入字符，WERASE 擦除前一个词
    ISIG--当接受到字符 INTR, QUIT, SUSP, 或 DSUSP 时，产生相应的信号
    */
    opt.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); 
    opt.c_cc[VMIN] = vmin;    //设置非规范模式下的超时时长和最小字符数：阻塞模式起作用
    opt.c_cc[VTIME] = vtime; //VTIME与VMIN配合使用，是指限定的传输或等待的最长时间 单位：0.1S
        
    tcflush (fd, TCIFLUSH);                 /* TCIFLUSH-- update the options and do it NOW */
    return (tcsetattr (fd, TCSANOW, &opt)); /* TCSANOW--改变立即发生 */
}

typedef enum
{
    kRxStateDataHeader1 = 0,
    kRxStateDataHeader2,
    kRxStateDataID,
    kRxStateDataLength1,
    kRxStateDataLength2,
    kRxStateSemDataLength1,
    kRxStateSemDataLength2,
    kRxStateDataBody,
    kRxStateDataCRC,
    kRxStateDataTrailer1,
    kRxStateDataTrailer2,
}RxState_t;

std::vector<uint8_t> rx_data_;
RxState_t rx_state_;
uint16_t rx_data_length_;

void DataLinkParse(uint8_t *buf, int length)
{
    for (int i = 0; i < length; i++)
    {
        const uint8_t data = buf[i];
        rx_data_.push_back(data);
        switch (rx_state_)
        {
            case kRxStateDataHeader1:
                if (data == 0xAA)
                {
                    rx_state_ = kRxStateDataHeader2;
                }
                else
                {
                    ResetRxState();
                }
                break;

            case kRxStateDataHeader2:
                if (data == 0x55)
                {
                    rx_state_ = kRxStateDataID;
                }
                else if (data == 0xAA)
                {
                    rx_data_.pop_back();
                }
                else
                {
                    ResetRxState();
                }
                break;
            
            case kRxStateDataID:
                rx_state_ = kRxStateDataLength1;
                break;

            case kRxStateDataLength1:
                rx_state_ = kRxStateDataLength2;
                break;

            case kRxStateDataLength2:
                rx_data_length_ = (rx_data_[3] << 8) | rx_data_[4];
                if (rx_data_length_ < 1 || rx_data_length_ > 255)
                {
                    ResetRxState();
                }
                else
                {
                    rx_state_ = kRxStateSemDataLength1;
                }
                break;
            
            case kRxStateSemDataLength1:
                rx_state_ = kRxStateSemDataLength2;
                break;

            case kRxStateSemDataLength2:
                rx_state_ = kRxStateDataBody;
                break;

            case kRxStateDataBody:
                if (rx_data_.size() == (rx_data_length_ + 7U))
                {
                    rx_state_ = kRxStateDataCRC;
                }
                break;

            case kRxStateDataCRC:
                if (VerifyOneFrame())
                {
                    rx_state_ = kRxStateDataTrailer1;
                }
                else
                {
                    ResetRxState();
                }
                break;

            case kRxStateDataTrailer1:
                if (data == 0xCC)
                {
                    rx_state_ = kRxStateDataTrailer2;
                }
                else
                {
                    ResetRxState();
                }
                break;
            
            case kRxStateDataTrailer2:
                if (data == 0x33)
                {
                    ResetRxState();
                }
                else
                {
                    ResetRxState();
                }
                break;

            default:
                ResetRxState();
                break;
        }
    }
}

void ResetRxState(void)
{
    rx_state_ = kRxStateDataHeader1;
    rx_data_length_ = 0;
    if (rx_data_.size() > 0)
    {
        rx_data_.clear();
    }
}

bool VerifyOneFrame(void)
{
    int data_crc = 0;
    int calc_crc = 0;
    data_crc = *(rx_data_.end() - 1);
    calc_crc = CalBufCrc8(&rx_data_[0], rx_data_.size() - 1);
    if (calc_crc != data_crc)
    {
        printf("CRC8 error:0x%02x, 0x%02x.\n", calc_crc, data_crc);
        for(long unsigned int i = 0; i < rx_data_.size(); i++)
        {
            printf("%02x ",rx_data_[i]);
        }
        printf("\n");
        return false;
    }
    return true;
}

static int made_table_ = 0;
static uint8_t crc8_table_[256];

void InitCrc8(void)
{
    int i, j;
    uint8_t crc;
    if(!made_table_){
        for(i = 0; i < 256; i++){
            crc = i;
            for(j = 0; j < 8; j++)
                crc = (crc << 1)^((crc & 0x80) ? DI : 0);
            crc8_table_[i] = crc & 0xFF;
        }
        made_table_ = 1;
    }
}

void Crc8(uint8_t *crc, const uint8_t m)
{
    if(!made_table_){
        InitCrc8();
    }
    *crc = crc8_table_[(*crc) ^ m];
    *crc &= 0xFF;
}

uint8_t CalBufCrc8(const uint8_t *inBuf, uint32_t inBufSz)
{
    uint8_t crc_base = 0;
    uint32_t i;
    for(i = 0; i < inBufSz; i++){
        Crc8(&crc_base, inBuf[i]);
    }
    return crc_base;
}


int main()
{
    int fd1;
    int ret;
    int rxlen = 0;
    fd_set rfds;
    struct timeval tv;
    int retval;
 
    fd1 = open(USAR1, O_RDWR | O_NOCTTY );//阻塞式读写             非阻塞| O_NDELAY | O_NONBLOCK
    if (fd1 < 0) 
    {
      perror("open uart1 error\n");
    }
    ret = set_port_attr(fd1, B230400, 8, "1", 'N',1,255);    /* 230400 8n1      */
    if(ret < 0) 
    {
      printf("set uart1 arrt faile \n");
      exit(-1);
    }
    
    while(1)
    {
       FD_ZERO(&rfds);
       FD_SET(fd1, &rfds);
       tv.tv_sec = 1;                           //in block mode is not used
       tv.tv_usec = 0;
       ret = select(fd1 + 1, &rfds, NULL, NULL, &tv);  //block mode
       if(ret > 0)
       {
           if(FD_ISSET(fd1,&rfds))
           {
               rxlen = read(fd1, buf, 255);
               if(rxlen > 0)
               {
                //  printf("len = %d\n\r",rxlen);
                 for(int i = 0; i < rxlen; i++)
                 {
                    printf("%02x ",buf[i]);
                 } 
                 printf("\n");        
                 DataLinkParse(buf, rxlen);        
               }
           }
       }     
    }
    return 0;
}
